#include "Path.h"
#include "heuristics.h"

Path::Path(void)
{
	_nodes.clear();
}


Path::~Path(void)
{
	_nodes.clear();
}

int Path::getLength()
{
	int len = 0;
	for (size_t i=1;i<_nodes.size();++i)
	{
		len = len + euclidian(_nodes[i],_nodes[i-1]);
	}
	return len;
}

void Path::addNode( AStarNode* n )
{
	if(n)
		_nodes.push_back(n);
}

void Path::fill()
{
	int N = _nodes.size();
	if (N==0)
		return;
	int i = 1;
	int xi,yi,dx,dy;
	int incrX,incrY;
	while (1)
	{
		xi = _nodes[i]->x;
		yi = _nodes[i]->y;
		dx = xi - _nodes[i-1]->x;
		dy = yi - _nodes[i-1]->y;
		if (abs(dx)>1 || abs(dy)>1)
		{
			incrX = dx/MAX(abs(dx),1);
			incrY = dy/MAX(abs(dy),1);
			//t_insert(self._nodes, i, self._grid:getNodeAt(self._nodes[i-1]._x + incrX, self._nodes[i-1]._y +incrY));
			N++;				
		}
		else
			i++;
		if (i>N)
			break;
	}
}

void Path::filter()
{
	if (_nodes.size()<=1)
		return;
	size_t i = 1;
	int xi,yi,dx,dy,olddx,olddy;
	xi = _nodes[i]->x;
	yi = _nodes[i]->y;
	dx = xi - _nodes[i-1]->x;
	dy = yi - _nodes[i-1]->y;
	std::vector<AStarNode*>::iterator it = _nodes.begin();
	it++;
	while(1)
	{
		olddx = dx;
		olddy = dy;
		if (++i<_nodes.size())
		{
			it++;
			xi = _nodes[i]->x;
			yi = _nodes[i]->y;
			dx = xi - _nodes[i-1]->x;
			dy = yi - _nodes[i-1]->y;
			if (olddx == dx && olddy == dy)
			{
				//t_remove(self._nodes, i-1)
				it = _nodes.erase(--it);
				i--;				
			}
		}
		else
			break;
	}
}

Path* Path::clone()
{
	Path* p = new Path();
	p->_nodes = this->_nodes;
	return p;
}

bool Path::isEqualTo( Path* p )
{
	Path* p1 = this->clone();
	p1->filter();
	Path* p2 = p->clone();
	p2->filter();
	size_t s1 = p1->_nodes.size();
	size_t s2 = p2->_nodes.size();
	if (s1 != s2)
		return false;
	for (size_t i=0;i<s1;++i)
	{
		AStarNode* n1 = p1->_nodes[i];
		AStarNode* n2 = p2->_nodes[i];
		if (n1->x != n2->x || n1->y != n2->y)
			return false;		
	}
	return true;
}

void Path::reverse()
{
	std::vector<AStarNode*> tmp;
	tmp.clear();
	std::vector<AStarNode*>::const_reverse_iterator it=_nodes.rbegin();
	while(it != _nodes.rend())
	{
		tmp.push_back(*it);
		++it;
	}
	_nodes.clear();
	_nodes = tmp;
}

void Path::append( Path* p )
{
	if(!p) return;
	std::vector<AStarNode*>::const_iterator it = p->_nodes.begin();
	while(it != p->_nodes.end())
	{
		this->addNode(*it);
		++it;
	}
}
